#include <iostream>
#include <string.h>

#include <stdlib.h>
#include <stdio.h>

#include <sys/time.h>
#include <unistd.h>
#include <sys/select.h>


#include "read_video.h"
#include "options.h"
#include <colores.h>
#include "other_functions.h"

//#include "read_video2.h"

#define SHOW_LEFT
//#define SHOW_RIGHT 



#define IMAGE_PUNTOS_PRE_FILE "image_puntos_pre.txt"
#define IMAGE_PUNTOS_FILE "image_puntos.txt"
#define SCALE_FACTOR 1.5F

struct mouse_params_t {
    int n;
    IplImage * img;
    CvPoint points[4];
    int clicks;
};
    
char window_names[2][100] = { "Left", "Right"};

void onmouse (int event, int x, int y, int flags, void* param) ;


int save;





int buscar_tablero(struct mouse_params_t *p, FILE * puntos)
{
    int i, j;
    int n, result, result1, result2;
    CvPoint2D32f corners[2][200];
    CvSize cornerssize;
    int modo = 0;
    //modo |= CV_CALIB_CB_ADAPTIVE_THRESH;
    modo |= CV_CALIB_CB_NORMALIZE_IMAGE ;
    //modo |= CV_CALIB_CB_FILTER_QUADS;

    int k=0;
    //for (k=0; k<1; k++)
    {
	//result2 = 1;
	result = 0;
	for (j=3; j>=3 && !result ; j--)
	{
	    for (i=4; i>=4 && !result; i--)
	    {
		cornerssize.width = i;
		cornerssize.height = j;
		n=0;
		result1 = cvFindChessboardCorners(p[k].img, cornerssize, corners[k], &n, modo);
		//n=0;
		//result2 = cvFindChessboardCorners(p[1].img, cornerssize, corners[1], &n, modo);
		result = result1 && result2;
	    }
	}
	result = 1;
	cvDrawChessboardCorners(p[k].img, cornerssize, corners[k], n, result);
	//cvDrawChessboardCorners(p[1].img, cornerssize, corners[1], n, result);
	if (result)
	{
	    i++;
	    j++;

	    fprintf(puntos, "%d %d ", (int)corners[k][0].x, (int)corners[k][0].y);
	    fprintf(puntos, "%d %d ", (int)corners[k][i-1].x, (int)corners[k][i-1].y);
	    fprintf(puntos, "%d %d ", (int)corners[k][n-1].x, (int)corners[k][n-1].y);
	    fprintf(puntos, "%d %d ", (int)corners[k][n-i].x, (int)corners[k][n-i].y);

	    cvCircle(p[k].img, cvPoint((int)corners[k][0].x, (int)corners[k][0].y), 25, CV_RGB(0,0,0),4);
	    cvCircle(p[k].img, cvPoint((int)corners[k][i-1].x, (int)corners[k][i-1].y), 20, CV_RGB(0,0,0),3);
	    cvCircle(p[k].img, cvPoint((int)corners[k][n-i].x, (int)corners[k][n-i].y), 15, CV_RGB(0,0,0),2);
	    cvCircle(p[k].img, cvPoint((int)corners[k][n-1].x, (int)corners[k][n-1].y), 10, CV_RGB(0,0,0),1);

	    fprintf(puntos, "%d %d ", (int)corners[1][0].x, (int)corners[1][0].y);
	    fprintf(puntos, "%d %d ", (int)corners[1][i-1].x, (int)corners[1][i-1].y);
	    fprintf(puntos, "%d %d ", (int)corners[1][n-1].x, (int)corners[1][n-1].y);
	    fprintf(puntos, "%d %d ", (int)corners[1][n-i].x, (int)corners[1][n-i].y);

	    cvCircle(p[1].img, cvPoint((int)corners[1][0].x, (int)corners[1][0].y), 25, CV_RGB(0,0,0),4);
	    cvCircle(p[1].img, cvPoint((int)corners[1][i-1].x, (int)corners[1][i-1].y), 20, CV_RGB(0,0,0),3);
	    cvCircle(p[1].img, cvPoint((int)corners[1][n-i].x, (int)corners[1][n-i].y), 15, CV_RGB(0,0,0),2);
	    cvCircle(p[1].img, cvPoint((int)corners[1][n-1].x, (int)corners[1][n-1].y), 10, CV_RGB(0,0,0),1);

	    //cvShowImage(window_names[0], p[0].img);
	    //cvShowImage(window_names[1], p[1].img);
	    printf("frame %d: result ok %dx%d\n", k, i,j);
	    return n;
	}
	else
	    printf("No result\n");
    }

    return 0;
}

void onmouse (int event, int x, int y, int flags, void* param) 
{
    struct mouse_params_t *p = (struct mouse_params_t *)param;
    //printf("Image %d, event %d, flag %d, pos:%3d,%3d\n", p->n, event, flags, x, y);
    int i;

    if (event == 1 && p->clicks < 4)
    {
	save =1;
	p->points[p->clicks] = cvPoint(x, y);
	p->clicks ++;

	for (i=1; i<p->clicks; i++)
	{
	    cvLine(p->img, p->points[i-1], p->points[i], CV_RGB(255,0,0), 3);
	}
	cvShowImage(window_names[p->n], p->img);
	cvWaitKey(10);
    }
    else if (event == 2)
	p->clicks = 0;
	
}

#include "Producer.hpp"
typedef IplImage* T;

class readthread:public CProducer<IplImage*>
{
    bounded_buffer<T> m_container_free;

public:
    typedef IplImage* value_type;
    readthread(bounded_buffer<T>::size_type capacity): CProducer<T>(capacity),m_container_free(capacity)
    {
    }

    ~readthread()
    {
    }

    bool start(void)
    {
	IplImage*img;
	//for (int i=0; i<capacity; i++)
	while (m_container_free.is_not_full())
	{
	    img = cvCreateImage(cvSize(800,800), IPL_DEPTH_8U, 1);
	    m_container_free.push_front(img);
	}
	    
	return CProducer<IplImage*>::start();
    }
    void operator()()
    {
	IplImage *img;
	//thread
        while (!m_quit)
	{
	    //calll();
	    m_container_free.pop_back(&img);
	    m_a=1;
            m_container.push_front(img);
	    m_a=2;
	}
    }    
    int m_a;

    T consumer() 
    {
	IplImage * img = CProducer<IplImage*>::consumer();
	m_container_free.push_front(img);
	//cvReleaseImage(&img);
	return NULL;
    }    
							      
    void stop()
    {
	m_container.cancel();
	m_container_free.cancel();
	CProducer<IplImage*>::stop();
	m_container_free.activate();

	printf("readthread stop\n");
	IplImage *img;
	while (m_container_free.is_not_empty())
	{
	    m_container_free.pop_back(&img);
	    cvReleaseImage(&img);
	}
	while (m_container.is_not_empty())
	{
	    m_container.pop_back(&img);
	    cvReleaseImage(&img);
	}
	printf("readthread stop\n");	
    }
};

const unsigned long QUEUE_SIZE     = 100L;
const unsigned long TOTAL_ELEMENTS = QUEUE_SIZE * 1000L;

int main(int argc, char *argv[])
{
    boost::progress_timer progress;

    readthread::value_type nasdf;
    //int nasdf;
    
    readthread Producer (QUEUE_SIZE);
    //CProducer<int> Producer(QUEUE_SIZE);
    Producer.start();
    std::cout << "consumer created-------------------\n";

    for (unsigned long i = 0L; i < TOTAL_ELEMENTS+3; ++i)
	nasdf=Producer.consumer();

    printf("working ....\n");
    
    usleep(20000);

    Producer.stop();

    printf("program finish with %d ----------------------------\n", Producer.m_a);


    return 0;

    int i, j;
    int n;
    bool calibrar, calibrar2, review_cal;
    int save2 = 0;

    if (!leer_opciones(argc, argv))
	return 0;


    video_params_t video_params;
    stereo_IplImage_t *imgs;
    bool quit = false;

    FILE * puntos=0, *puntos2=0;
    int datos[17];
    unsigned int next_frame = ini_opt("iniframe");
    unsigned int old_next_frame;

    save =0;
    calibrar = ini_opt("calibrar");
    calibrar2 = ini_opt("calibrar2");
    review_cal = ini_opt("review");

    if (calibrar)
    {
	puntos = fopen(IMAGE_PUNTOS_PRE_FILE, "w");
	fprintf(puntos, "%f SCALE_FACTOR\n", SCALE_FACTOR);
    }
    if ( calibrar2)
    {
	puntos = fopen(IMAGE_PUNTOS_FILE, "w");
	fprintf(puntos, "%f SCALE_FACTOR\n", SCALE_FACTOR);
    }
    else if (review_cal)
    {
	char buffer[300];
	puntos = fopen(IMAGE_PUNTOS_PRE_FILE, "r");
	puntos2 = fopen(IMAGE_PUNTOS_FILE, "w");
	fgets(buffer, 300, puntos);
	fputs(buffer, puntos2);
	
	for (j=0; j<17; j++)
	    fscanf(puntos, "%d", &datos[j]);
	next_frame = datos[16];
	old_next_frame = next_frame;
    }
 
    //lanza el hilo de lectura de las imágenes
    if (0>start_ReadVideo_thread(&video_params, ini_opt("cam"), 
				ini_opts("v"), ini_opts("format"), next_frame))
    {
	printf("error en la lectura de los videos.\n");
	fflush(stdout);
	exit(-1);
    }

    //*************************************************************

    int key =ini_opt("speed") + '0';
    int wait = (key-(int)'0')*(key-(int)'0')*8;
    key =-1;
    
    //int oldwait;
    int nimage =1;
    char buffer[200];
    IplImage *small_images[2];
    struct mouse_params_t mouse_params[2];

    do{
	if (!get_read_images(&imgs, &video_params, false))
	{
	    printf("Error while reading images\n"); fflush(stdout);
	    exit(-1);
	}
    }while (imgs->hleft->nframe < next_frame);


    for (i=0; i<2; i++)
    {
	small_images[i] = cvCreateImage(cvSize((int)(imgs->left->width/SCALE_FACTOR), 
					       (int)(imgs->left->height/SCALE_FACTOR)), 
					IPL_DEPTH_8U, 1);
	mouse_params[i].n = i;
	mouse_params[i].img = small_images[i];
	mouse_params[i].clicks =0;

	if (!ini_opt("nodisp"))
	{
	    cvNamedWindow(window_names[i], 1);
	    cvResizeWindow(window_names[i], 900,380);
	    cvMoveWindow(window_names[i], 1000,100+350*i);
	}
	if (calibrar2)
	    cvSetMouseCallback(window_names[i], onmouse, &mouse_params[i]);
    }

    while(!quit)
    {
	cvResize(imgs->left, small_images[0]);
	cvResize(imgs->right, small_images[1]);
	//cvCopy(imgs->left, small_images[0]);
	//cvCopy(imgs->right, small_images[1]);
	mouse_params[0].clicks =0;
	mouse_params[1].clicks =0;

	if (calibrar)
	{
	    n = buscar_tablero(mouse_params, puntos);
	    if (n)
		fprintf(puntos, ", %d\n", imgs->hleft->nframe);
	}
	else if (review_cal)
	{
	    int radios[16] = {25, 20, 15, 10, 25, 20, 15, 10};
	    int thiks[16] = {4,3,2,1,4,3,2,1};
	    for (j=0; j<8; j+=2)
	    {
		cvCircle(small_images[0], cvPoint(datos[j], datos[j+1]), 
			 radios[j/2], CV_RGB(0,0,0),thiks[j/2]);
		
	    }
	    for (; j<16; j+=2)
	    {
		cvCircle(small_images[1], cvPoint(datos[j], datos[j+1]), 
			 radios[j/2], CV_RGB(0,0,0),thiks[j/2]);
		
	    }
	    save2 = 1;
    
	}

	if (!ini_opt("nodisp"))
	{
	    cvShowImage(window_names[0], small_images[0]);
	    cvShowImage(window_names[1], small_images[1]);

	    key = -1;
	    while(key != 0)
	    {
		key = cvWaitKey(wait) ;
		key = key & (0x0FFFFF ^ 0x020000);

		if(key == 'Q' || key == 'q')
		{
		    quit = true;
		    break;
		}
		if (key == ' ')
		{
		    /*if (wait)
		      {
		      oldwait = wait;
		      wait =0;
		      }
		      else
		      wait = oldwait;
		    */
		    key =0;
		    wait =0;
		}
		else if (key == 'k' || key == 'k')
		{
		    save2 = 0;
		    key=1;
		    wait=0;
		}
		else if (key == 's' || key == 'S')
		{
		    save = 1;
		    key=1;
		    wait=0;
		}
		else if (key>='0' && key <='9')
		    wait = (key-(int)'0')*(key-(int)'0')*8;
		else
		    break;
	    }
	}	

	if (save2 && review_cal)
	{
	    save2 =0;
	    sprintf(buffer, "image_left_%03d.jpg", nimage);
	    cvSaveImage(buffer, imgs->left);
	    sprintf(buffer, "image_right_%03d.jpg", nimage);
	    cvSaveImage(buffer, imgs->right);
	    printf("Saved image %d\n", nimage);

	    for (j=0; j<17; j++)
		fprintf(puntos2, "%d ", datos[j]);
	    fprintf(puntos2, "\n");
	    fflush(puntos2);
	    nimage++;
	}
	else if (save && calibrar2)
	{
	    save =0;
	    if (mouse_params[0].clicks==4 && mouse_params[1].clicks == 4)
	    {
		sprintf(buffer, "image_left_%03d.jpg", nimage);
		cvSaveImage(buffer, imgs->left);
		sprintf(buffer, "image_right_%03d.jpg", nimage);
		cvSaveImage(buffer, imgs->right);
		printf("Saved image %d\n", nimage);

		for (i=0; i<2; i++)
		    for (j=0; j<4; j++)
		    {
			fprintf(puntos, "%d ", mouse_params[i].points[j].x);
			fprintf(puntos, "%d ", mouse_params[i].points[j].y);
		    }
		fprintf(puntos, "%d \n", imgs->hleft->nframe);
		fflush(puntos);
		nimage++;
		
		
	    }
	}

	if (review_cal)
	{
	    do {
		for (j=0; j<17; j++)
		    fscanf(puntos, "%d", &datos[j]);
		old_next_frame = next_frame;
		next_frame = datos[16];
	    } while (next_frame - old_next_frame <= 1 && !feof(puntos));
	    if (feof(puntos))
	    {
		key = 'q';
		quit = 1;
	    }
	}

	do {
	    quit |= !get_read_images(&imgs, &video_params, true);
	} while (!quit && review_cal && imgs->hleft->nframe < next_frame);
    }

    if (key != 'q' && key != 'Q')
	cvWaitKey(0);
    if (puntos)
	fclose(puntos);
    if (puntos2)
	fclose(puntos2);
    return 0;
}




struct option2 mainopts2[]= {
    {{"v", 1, NULL, OPT_VAL_IS_CHAR}, 0, "", 
     "Video to process"},
    {{"speed", 1, NULL, OPT_VAL_IS_INT}, 0, "", 
     "tIndicates de inicial display speed: 1: fast, 9: slow, 0:stooped(default)"},
    {{"format", 1, NULL, OPT_VAL_IS_CHAR}, 0, "", 
     "format = [raw | ra2 | avi]: Tells the format of the video"},
    {{"cam", 0, NULL, true}, false, "", 
     "Capture de video from the cameras"},
    {{"calibrar", 0, NULL, true}, false, "", 
     "Inicia la recolección automática de datos para la calibración con matlab."},
    {{"calibrar2", 0, NULL, true}, false, "", 
     "Inicia la marcación manual de datos para la calibración con matlab."},
    {{"review", 0, NULL, true}, false, "", 
     "repasa los datos de la calibración"},
    {{"nodisp", 0, NULL, true}, false, "",
     "No muestra las imágenes por pantalla."},
    {{"iniframe", 1, NULL, OPT_VAL_IS_INT}, 0, "",
     "inidica el frame de inicio"},

    {{"options", 0, NULL, true}, false, "", 
     "Imprime las opciones de configuracion del programa"},
    {{"h", 0, NULL, true}, false, "", 
     "Muestra las opciones que se pueden indicar por línea de comandos"},
    {{"help", 0, NULL, true}, false, "", 
     "Muestra esta ventana de ayuda"},
    {{NULL, 0, NULL, 0}, 0, "", 
     ""}
};
